/**
    Copyright 2015 Table Rider
    By: Richard Solti
*/

#include "ControllerMotor.h"
#include "Definitions.h"
#include <cstdlib>
#include <stdio.h>

#define limit 300.0

ControllerMotor::ControllerMotor(Motor *m1, Motor *m2)
{
	left = m1;
	right = m2;
    oldPosL = left->getEncoder();
    oldPosR = right->getEncoder();

    posP        = 0.002;//0.00125; //0.0015/0.001  Denna parameter sänker vi för att få mjukare inbromsning
    speedP      = 0.035; // 0.01 ------ÖKTE
    speedI      = 0.12;//0.002  //0.002
    currP       = 7000;//2200
    cLoop       = 1;
    sLoop       = 10;
    pLoop       = 100;//Numbers are in 0.1 milliseconds
    treshold = 100; //100
    victory = 1;
    acceleration = 0.01;
    syncer = 10000;// -------- SÄNKTE FRÅN 7500
    clampVal = AVE_PULSE_P_SEC * 0.001 * SAMPLE_INTERVAL * sLoop;
    //printf("posP: %f speedP: %f speedI: %f currP: %d \n", posP, speedP, speedI, currP);
}

void ControllerMotor::convertAD()
{
    //Start an ADC in ADC_CR
    REG_ADC_CR = 1<<1;
    //Wait until IDR_DRDY
    while((REG_ADC_ISR & (1<<24)) != (1<<24))
    {
        asm("nop");
    }
}

long t1, t2, t3;
void ControllerMotor::init(long a, long b)
{
    if((a < 0 && b > 0) || (a > 0 && b < 0))
       treshold = 5;
    else
       treshold = 100;
    dPosL       = a + left->getEncoder();
    dPosR       = b + right->getEncoder();
    oldPosL     = left->getEncoder();
    oldPosR     = right->getEncoder();
    speedSumL   = 0;
    speedSumR   = 0;
    t1 = 1;
    t2 = 1;
    t3 = 1;
    victory = 0;
    limiter = 0;
    REG_PIOC_SODR = 1<<SLEEP1 | 1<<SLEEP2;
}

void ControllerMotor::update()
{
        if(t1 <= ticks && !victory)
        {
            t1 = ticks + pLoop;
            positionLoop();
        }
        if(t2 <= ticks && !victory)
        {
            t2 = ticks + sLoop;
            speedLoop();
        }
        if(t3 <= ticks && !victory)
        {
            t3 = ticks + cLoop;
            currentLoop();
        }
}

void ControllerMotor::positionLoop()
{
    long posErrL = (dPosL - left->getEncoder());
    long posErrR = (dPosR - right->getEncoder());
    if(abs((int)posErrL) < treshold && abs((int)posErrR) < treshold)
    {
        victory = 1;
        left->setDuty(0);
        right->setDuty(0);
        REG_PIOC_CODR = 1<<SLEEP1 | 1<<SLEEP2;
        return;
    }

    if(limiter < 1.0)
    {
        limiter += acceleration;
        if(limiter >= 1.0)
            limiter = 1;
    }
    double limitL = 1, limitR = 1;
    int diff = abs(posErrL) - abs(posErrR);
    if(diff < 0)
    {
        limitL = clamp(0.0, 1 - ((double)abs(diff) / syncer), 1.0);
    }
    else if (diff > 0)
    {
        limitR = clamp(0.0, 1 - ((double)abs(diff) / syncer), 1.0);
    }
    dSpeedL = clamp(-(double)clampVal, (double)posErrL * posP, (double)clampVal) * limiter * limitL;
    dSpeedR = clamp(-(double)clampVal, (double)posErrR * posP, (double)clampVal) * limiter * limitR;
}

void ControllerMotor::speedLoop()
{
    double speedErrL = dSpeedL - (left->getEncoder() - oldPosL);
    double speedErrR = dSpeedR - (right->getEncoder() - oldPosR);
    oldPosL = left->getEncoder();
    oldPosR = right->getEncoder();
    speedSumL = clamp(-0.8 / speedI, speedErrL / 10.0 + speedSumL, 0.8 / speedI);
    speedSumR = clamp(-0.8 / speedI, speedErrR / 10.0 + speedSumR, 0.8 / speedI);
    dCurrL = clamp(-0.8, speedP * (speedErrL) + speedI * speedSumL, 0.8);
    dCurrR = clamp(-0.8, speedP * (speedErrR) + speedI * speedSumR, 0.8);
    ////////////////////////////////////////////////////////////////////////////////////////////
    //dCurrL = speedP * speedErrL + speedI * speedSumL;
    //dCurrR = speedP * speedErrR + speedI * speedSumR;
    //int timp = currP * dCurrL;
    //left->setDuty(timp);//Duty begränsas automatiskt mellan -4200 och 4200
    //int timp2 = currP * dCurrR;
    //right->setDuty(timp2);//Riktning ställs automatiskt i Motor klassen om duty är negativ/positiv
    ////////////////////////////////////////////////////////////////////////////////////////////

}

void ControllerMotor::currentLoop()
{
    dconvertAD();
    double tempL = left->getRealCurrent();   //3.3 / 4095 / 4 / 1000) * 7000 från tommy, där 3.3 är volt,
    double tempR = right->getRealCurrent();//0.00141;  //4095 upplösningen, 4 = PGA, I_out/I_sense = 7000 och R_sense = 1000
    if(dCurrL < 0)
        tempL = -tempL;
    if(dCurrR < 0)
        tempR = -tempR;
    int timp = currP * clamp((dCurrL < 0)? dCurrL : 0, dCurrL + (dCurrL - tempL),(dCurrL < 0)? 0 : dCurrL);
    left->setDuty(timp);//Duty begränsas automatiskt mellan -4200 och 4200
    int timp2 = currP * clamp((dCurrR < 0)? dCurrR : 0,dCurrR + (dCurrR - tempR),(dCurrR < 0)? 0 : dCurrR);
    right->setDuty(timp2);//Riktning ställs automatiskt i Motor klassen om duty är negativ/positiv

}


int ControllerMotor::done()
{
    if(victory)
        return 1;
    return 0;
}
